#include <tT.h>
#include <env.h>

#define MS 16

int m0[MS][MS];
int m1[MS][MS];
int r[MS][MS];

void matrix_mult(int m0[MS][MS], int m1[MS][MS], int r[MS][MS])
{
	int x, y, k;
	TIM0->TCR = 1;
	for (x=0;x<MS;x++) {
		for (y=0;y<MS;y++) {
			r[x][y] = 0;
			for (k=0;k<MS;k++) {
				r[x][y] += m0[x][k] * m1[k][y];
			}
		}
	}
}

/* ************************************************************************** */

#define AS 128
int array0[AS];

int array_sum(int a0[AS])
{
	int i, sum = 0;
	for (i=0;i<AS;i++) {
		sum += a0[i];
	}

	return sum;
}

/* ************************************************************************** */

typedef struct test_t
{
	tt_object_t super;
	int mult;
	int sum;
} test_t;
#define test() {tt_object()}

static test_t test = test();

static env_result_t test_meth(test_t *this, void *args)
{
	return 0;
}

static env_result_t test_mult(test_t *this, void *args)
{
	TIM0->TCR = 1;
	if (this->mult--) {
		TT_AFTER_BEFORE(100000, 100000,
			&test, test_mult, &tt_args_none
			);
	}
	matrix_mult(m0, m1, r);
	return 0;
}

/* ************************************************************************** */

static env_result_t test_sum(test_t *this, void *args)
{
	if (this->sum--) {
		TT_AFTER_BEFORE(
			100000, 50000,
			&test, test_sum, &tt_args_none
			);
	}
	return array_sum(array0);
}

/* ************************************************************************** */

static env_result_t clock_halt(test_t *this, void *args)
{
	TIM0->TCR = 0;
	return 0;
}

/* ************************************************************************** */

typedef struct dummy_t
{
	tt_object_t super;
} dummy_t;
#define dummy() {tt_object()}

static dummy_t dummy = dummy();

static env_result_t dummy_meth(dummy_t *this, void *args)
{
	/* Do nothing, it's a dummy after all... */
	return 0;
}

/* ************************************************************************** */

typedef struct wait_t
{
	tt_object_t super;
	volatile int release;
} wait_t;
#define wait() {tt_object(), 0}

static wait_t wait = wait();

static env_result_t wait_for(wait_t *this, void *args)
{
	while (!this->release);
	return 0;
}

/* ************************************************************************** */

typedef struct counter_t
{
	tt_object_t super;
	uint32_t    count;
} counter_t;
#define counter() {tt_object(), 0}

static counter_t counter = counter();

static env_result_t counter_inc(counter_t *this, void *args)
{
	this->count += 1;
	return 0;
}

static env_result_t counter_dec(counter_t *this, void *args)
{
	this->count -= 1;
	return 0;
}

/* Removing this will lower the priority-ceiling to 1 for wait.
static env_result_t counter_inc(counter_t *this, void *args)
{
	this->count += 1;
	TT_SYNC(&wait, wait_for, &tt_args_none);
	return 0;
}
*/

/* Timer0 interrupt handler. */
void TIMER0_IRQHandler(void)
{
	/* Required for correct timestamp... */
	tt_interrupt_mark();

	/* Clear interrupt. */
	TIM0->IR = (1 << 0);

	/*TT_BEFORE(325, &test, clock_halt, &tt_args_none);*/

	/* This should not be executed, only scheduled... */
	TT_BEFORE(1000, &dummy, dummy_meth, &tt_args_none);
	/*TT_BEFORE(200000, &dummy, dummy_meth, &tt_args_none);*/

	/* Schedule is always required. */
	tt_schedule(1);
}

static void init(void)
{
	int i;

	/* Initialize objects manually. */
	TT_OBJECT_CEIL(&test, 1);
	TT_OBJECT_CEIL(&dummy, 2);
	TT_OBJECT_CEIL(&wait, 1);
	TT_OBJECT_CEIL(&counter, 2);

#if 0
	/*
	 * Usually we dont want the timer to start before we leave the init
	 * function (semantic thing) but we need it to be able to measure
	 * the time e.g. for inserting items in the run/timer queue.
	 */
	ENV_TIMER_START();
#endif

#if 0
	/*
	 * This is used to measure the time it takes to enqueue something
	 * into the runQueue.
	 */
	for (i=0;i<TT_NUM_MESSAGES;i++) {
		TT_AFTER_BEFORE(
			0, (200000-(4000*i)),
			&test, test_meth, &tt_args_none
			);
	}
#endif

#if 0
	/*
	 * This is used to measure the time it takes to enqueue something
	 * into the timerQueue.
	 */
	for (i=0;i<TT_NUM_MESSAGES;i++) {
		TT_AFTER_BEFORE(
			(200000-(4000*i)), 10000,
			&test, test_meth, &tt_args_none
			);
	}
#endif

#if 0
	/*
	 * This is used to measure the overhead of n postponed messages.
	 */
	for (i=0;i<TT_NUM_MESSAGES;i++) {
		TT_AFTER_BEFORE(
			100000, (200000-(4000*i)),
			&test,
			test_meth,
			&tt_args_none
			);
	}
#endif

	/*
	 * This is used to measure the overhead of an external event.
	 */
	
	/* Enable power to timer0 and clock it using CCLK. */
	SC->PCONP    |= (1 << 1);
	SC->PCLKSEL0 &= ~(0x3 << 2);
	SC->PCLKSEL0 |= (0x1 << 2);

#if 1
	/* Setup timer interval. */
	TIM0->TCR = 0;
	TIM0->CTCR = 0;
	TIM0->PR = 0;
	TIM0->TC = 0;
	TIM0->MCR = (1 << 0) | (1 << 1) | (1 << 2);
	TIM0->MR0 = 40000;
	NVIC_EnableIRQ(TIMER0_IRQn);
#endif

#if 0
	for (i=0;i<(sizeof(array0)/sizeof(array0[0]));i++) {
		array0[i] = i;
	}

	test.sum = 40;
	TT_AFTER_BEFORE(
		125000, 50000,
		&test, test_sum, &tt_args_none
		);
#endif

#if 1
	for (i=0;i<(sizeof(m0)/sizeof(int));i++) {
		((int *)m0)[i] = i;
	}
	for (i=0;i<(sizeof(m1)/sizeof(int));i++) {
		((int *)m1)[i] = i;
	}
	for (i=0;i<(sizeof(r)/sizeof(int));i++) {
		((int *)r)[i] = i;
	}

	test.mult = 40;
	TT_AFTER_BEFORE(
		100000, 100000,
		&test, test_mult, &tt_args_none
		);
#endif

#if 0
	TT_AFTER_BEFORE(
		10000, 10000,
		&test, test_meth, &tt_args_none
		);
#endif

#if 0
	/*
	 * Regular testbed.
	 */
	TT_AFTER_BEFORE(
		1, 10000,
		&wait, wait_for, &tt_args_none
		);

	TT_AFTER_BEFORE(
		1000, 100,
		&counter, counter_dec, &tt_args_none
		);
#endif
}

ENV_STARTUP(init);
